/* Teach Tool  - Structural Dynamics
 * Copyright (C) 2009: Fabian Gerold, www.fabiangerold.de
 *
 * This file may be licensed under the terms of of the
 * GNU General Public License Version 3 (the "GPL").
 * 
 * Software distributed under the License is distributed 
 * on an "AS IS" basis, WITHOUT WARRANTY OF ANY KIND, either 
 * express or implied. See the GPL for the specific language 
 * governing rights and limitations.
 *
 * You should have received a copy of the GPL along with this 
 * program. If not, go to http://www.gnu.org/licenses/gpl.html
 * or write to the Free Software Foundation, Inc., 
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/

#include <QTimer>
#include <private/qmath_p.h>
#include <assert.h>
#include "System_harm.h"
#include "window_harm.h"
#include "../src_common/Diagram.h"
#include "../src_common/EMSWidget.h"

const float Pi = 3.141592653589793f;

SystemHarm::SystemHarm()
{
	connect( &m_timer, SIGNAL( timeout() ), this, SLOT( slotFrame() ) );
	m_timer.start( 10 );
	m_halted = true;
	m_paint = true;
	m_buff_output = false;

	m_k = 5000;
	m_m = 25;
	m_xi = 0.05;
	m_amp = 0;
	m_diag_scale = 1.0f;

	reset();

	m_mode = 0;
	
	m_F0 = 1000;
	m_omega_F = 1.0*2.0*Pi;

	m_G0 = 0.1;
	m_omega_G = 1.0*2.0*Pi;

	// newmark
	m_dt = 0.01;
	m_delta = 0.5;
	m_alpha = 0.25; //0.3*( 0.5 + nm_delta )*( 0.5 + nm_delta );

	m_a0 = 1/( m_alpha*m_dt*m_dt );
	m_a1 = m_delta/( m_alpha*m_dt );
	m_a2 = 1/( m_alpha*m_dt );
	m_a3 = 1/( 2*m_alpha ) - 1;
	m_a4 = m_delta/m_alpha -1;
	m_a5 = m_dt/2.0*( m_delta/m_alpha - 2 );
	m_a6 = ( 1 - m_delta )*m_dt;
	m_a7 = m_delta*m_dt;

	m_txt_out = 0;
	m_stop_output = 0;

	parameter();
}

void SystemHarm::parameter()
{
	m_T = 2*Pi*qSqrt( m_m/m_k ); //m_T = 2*Pi*sqrt( m_m/m_k );
	m_f = 1.0/m_T;
	m_omega = 2*Pi*m_f;
	
	//m_omega_d = sqrt( 1 - m_xi*m_xi )*m_omega;
	if( m_xi < 1 ) m_omega_d = sqrt(1 - m_xi*m_xi)*m_omega;
	else m_omega_d = 0.9999;

	m_fi = -atan(1/m_xi);
	m_c = 2*m_m*m_omega*m_xi;

	emit( signalUpdateParameter( m_T, m_f, m_omega, m_omega_d ) );
}

void SystemHarm::slotStart( double )
{
	reset();

	double max_u = 0;
	double max_up = 0;
	double max_upp = 0;

	// pre-calculate max u, up, upp
	for( int i=0; i<2000; i++ )
	{
		step();
		m_timestep++;
		m_sec += 0.01;

		if( fabs( m_u )		> max_u		)	max_u	= fabs( m_u );
		if( fabs( m_up )	> max_up	)	max_up	= fabs( m_up );
		if( fabs( m_upp )	> max_upp	)	max_upp = fabs( m_upp );
	}
	reset();

	emit( signalClearPoints() );


	emit( signalDiagSetMax( max_u, max_up, max_upp ) );
	//m_diag->setMax( max_u, max_up, max_upp );

	// prepare to start
	m_halted = false;
	m_buff_output = false;
	m_paint = true;
	m_stop_output = 0;

}

void SystemHarm::slotStop()
{
	m_halted = true;
}

void SystemHarm::proceed()
{
	m_halted = false;
}

float SystemHarm::addScale( float scale )
{
	if( m_diag_scale+scale >= 0.4 && m_diag_scale+scale <= 5 )
	{
		m_diag_scale += scale;
	}

	emit( signalDiagSetScale( m_diag_scale ) );
	return m_diag_scale;
}

void SystemHarm::reset()
{
	m_halted = true;
	m_timestep = 0;
	m_stop_output = 0;
	m_sec = 0.0;
	m_u = 0;
	m_u1 = 0;
	m_up = 0;
	m_up1 = 0;
	m_upp = 0;
	m_upp1 = 0;

	m_force = 0;
	m_ub = 0;
	m_ub1 = 0;

	emit( signalClearPoints() );
	emit( signalDiagUpdate() );
	emit( signalResetDisp() );
	emit( signalUpdateTime( m_sec ) );
	emit( signalClearOutput() );
}

//void SystemHarm::showCurves( bool u, bool up, bool upp )
//{
//	m_diag->showCurves( u, up, upp );
//	m_diag->totalRepaint();
//}

void SystemHarm::setTxtOutput( int out )
{
	m_txt_out = out;
}

void SystemHarm::slotFrame()
{
	if( m_halted ) return;

	step();

	// text output
	if( ( m_txt_out == 1 && m_timestep%10 == 0 ) || m_txt_out == 2 )
	{
		if( m_u < 0.0001 && m_up < 0.0001 && m_upp < 0.0001 )
		{
			m_stop_output++;
		}

		if( m_stop_output < 4 )
		{
			if( m_mode == 0 ) // force
				m_output = QString::number( m_sec ) + "\t" 
				+ QString::number( m_force, 103, 4 ) + "\t" 
				+ QString::number( m_u, 103, 3 ) + "\t" 
				+ QString::number( m_up, 103, 3 ) + "\t" 
				+ QString::number( m_upp, 103, 3 );
			else
				m_output = QString::number( m_sec ) + "\t" 
				+ QString::number( m_ub, 103, 3 ) + "\t" 
				+ QString::number( m_u, 103, 3 ) + "\t" 
				+ QString::number( m_up, 103, 3 ) 
				+ "\t" + QString::number( m_upp, 103, 3 );

			if( m_txt_out == 1 )
			{
				emit( signalAppendOutput( m_output ) );
			}
			else
			{
				if( m_buff_output )
				{
					emit( signalAppendOutput( m_output_buff ) );
					emit( signalAppendOutput( m_output ) );
				}
				else 
				{
					m_output_buff = m_output;
				}
				m_buff_output = !m_buff_output;
			}
		}
	}

	emit( signalDiagAddPoint( m_sec, m_u, m_up, m_upp ) );
	//m_diag->addPoint( m_sec, m_u, m_up, m_upp );
	m_timestep++;
	m_sec += 0.01;

	if( m_paint )
	{
		emit( signalUpdateTime( m_sec ) );
		//m_ems->setDisp( m_u, m_ub );
		emit( signalSetDisp( m_u, m_ub ) );
		emit( signalDiagUpdate() );
	}

	m_paint = !m_paint;
}

void SystemHarm::step()
{
//	m_timestep++;
//	m_sec += 0.01;

	//double e = exp( -m_xi*m_omega_d*m_sec );
	//double sinus = sin( m_omega_d*m_sec - m_fi );
	//double cosinus = cos( m_omega_d*m_sec - m_fi );
	
	m_u2 = m_u1;
	m_u1 = m_u;
	//m_u = m_amp*e*sinus;

	m_up2 = m_up1;
	m_up1 = m_up;
	//m_up = m_amp*m_omega_d*e*( -m_xi*sinus + cosinus );

	m_upp2 = m_upp1;
	m_upp1 = m_upp;
	//m_upp = m_amp*m_omega_d*m_omega_d*e*( ( m_xi*m_xi-1 )*sinus - 2*m_xi*cosinus );

	if( m_mode == 0 )
	{
		// new force value
		m_force1 = m_force;
		m_force = m_F0*sin( m_omega_F*m_sec );
		if( m_paint )
		{
			//m_ems->setForce( m_force/m_F0 );
			emit( signalSetForce( m_force/m_F0 ) );
		}
		
		// newmark
		double nm_k = m_k + m_a1*m_c + m_a0*m_m;
		double nm_F = m_force;
		nm_F += m_m*( m_a0*m_u1 + m_a2*m_up1 + m_a3*m_upp1 );
		nm_F += m_c*( m_a1*m_u1 + m_a4*m_up1 + m_a5*m_upp1 );
		
		m_u = nm_F/nm_k;
		m_upp = m_a0*( m_u - m_u1 ) - m_a2*m_up1 - m_a3*m_upp1;
		m_up = m_up1 + m_a6*m_upp1 + m_a7*m_upp;
	}
	else if( m_mode == 1 )
	{
		m_ub1 = m_ub;
		m_ub = m_G0*sin( m_omega_G*m_sec );

		// keep the old values
		

		// newmark
		double nm_k = m_k + m_a1*m_c + m_a0*m_m;

		double nm_F = m_k*m_ub + m_c*(m_ub-m_ub1)/m_dt + m_m*( m_a0*m_u1 + m_a2*m_up1 + m_a3*m_upp1 );
		nm_F += m_c*( m_a1*m_u1 + m_a4*m_up1 + m_a5*m_upp1 );
			
		m_u = nm_F/nm_k;
		m_upp = m_a0*( m_u - m_u1 ) - m_a2*m_up1 - m_a3*m_upp1;
		m_up = m_up1 + m_a6*m_upp1 + m_a7*m_upp;
	}
		
}


void SystemHarm::setSpring( double k )
{
	m_k = k;
	emit( signalSetSpring( k ) );
	parameter();
}

void SystemHarm::setMass( double m )
{
	m_m = m;
	emit( signalSetMass( m ) );
	parameter();
}

void SystemHarm::setDamp( double d )
{
	m_xi = d*0.01;
	emit( signalSetDamp( d ) );
	parameter();
}

void SystemHarm::setAmp( double a )
{
	m_amp = a;
}

void SystemHarm::setMode( int mode )
{
	m_mode = mode;
	restoreExcitationPath();
}

void SystemHarm::restoreExcitationPath()
{
	QPainterPath* path = new QPainterPath;
	path->moveTo( 0, 0 );
	int max = 5000;
	//if( m_diag_scale < 1 ) max = 4000;
	//if( m_diag_scale < 0.8 ) max = 5000;
	if( m_mode == 0 ) // force
	{
		m_ub = 0;
		m_u1 = 0;
		//m_ems->setForceVisible( true );
		emit( signalSetForceVisible( true ) );

		// excitation path
		float f;
		float time = 0;
		int i;
		for( i=0; i<max; ++i )
		{
			f = -100*sin( m_omega_F*time );
			path->lineTo( i, f );
			time += 0.01f;
		}
		// close path
		float dt = 0.005f;
		while( f>1.0 )
		{
			f = -100*sin( m_omega_F*time );
			path->lineTo( i, f );
			time += dt;
			assert( i < max+20 );
			if( ++i > max+200 ) break;
		}
		dt = 0;
	}
	else
	{
		emit( signalSetForceVisible( false ) );

		// excitation path
		float g;
		float time = 0;
		int i;
		for( i=0; i<max; ++i )
		{
			g = -100*sin( m_omega_G*time );
			path->lineTo( i, g );
			time += 0.01f;
		}
		// close path
		float dt = 0.005f;
		while( g>1.0 )
		{
			g = -100*sin( m_omega_G*time );
			path->lineTo( i, g );
			time += dt;
			assert( i < max+20 );
			if( ++i > max+200 ) break;
		}
		dt = 0;
	}

	emit( signalSetExcitation( path, m_mode ) );
}

void SystemHarm::setOmegaF( double omega )
{
	m_omega_F = omega;
	restoreExcitationPath();
}

void SystemHarm::setF0( double f )
{
	m_F0 = f;
}

void SystemHarm::setOmegaG( double omega )
{
	m_omega_G = omega;
	restoreExcitationPath();
}

void SystemHarm::setG0( double a )
{
	m_G0 = a;
}

